package Tests
{
	import Box2D.Collision.b2AABB;
	import Box2D.Collision.b2Collision;
	import Box2D.Collision.b2DynamicTree;
	import Box2D.Collision.b2RayCastInput;
	import Box2D.Collision.b2RayCastOutput;
	import Box2D.Common.Math.b2Math;
	import Box2D.Common.Math.b2Vec2;
	import Box2D.Common.b2Color;
	import Box2D.Common.b2Settings;
	
	import Framework.Test;
	
	public class DynamicTreeTest extends Test
	{
		private static const e_actorCount:int = 128;
		
		private var m_worldExtent:Number;
		private var m_proxyExtent:Number;
		private var m_tree:b2DynamicTree = new b2DynamicTree;
		private var m_queryAABB:b2AABB = new b2AABB
		private var m_rayCastInput:b2RayCastInput = new b2RayCastInput;
		private var m_rayCastOutput:b2RayCastOutput = new b2RayCastOutput;
		private var m_rayActor:Actor;
		private var m_actors:Vector.<Actor> = new Vector.<Actor>(e_actorCount);
		private var m_automated:Boolean;
		
		public function DynamicTreeTest()
		{
			super();
			for(var i:int=0;i<m_actors.length;i++){
				m_actors[i] = new Actor;
			}
			
			m_worldExtent = 15.0;
			m_proxyExtent = 0.5;
			
			for (i=0; i < e_actorCount; ++i)
			{
				var actor:Actor = m_actors[i];
				GetRandomAABB(actor.aabb);
				actor.proxyId = m_tree.CreateProxy(actor.aabb, actor);
			}
			
			
			var h:Number = m_worldExtent;
			m_queryAABB.lowerBound.Set(-3.0, -4.0 + h);
			m_queryAABB.upperBound.Set(5.0, 6.0 + h);
			
			m_rayCastInput.p1.Set(-5.0, 5.0 + h);
			m_rayCastInput.p2.Set(7.0, -4.0 + h);
			m_rayCastInput.maxFraction = 1.0;
			
			m_automated = false;
		}
		private function QueryCallback(proxyId:int):Boolean{
			var actor:Actor = m_tree.GetUserData(proxyId) as Actor;
			actor.overlap = b2Collision.TestOverlap(m_queryAABB, actor.aabb);
			return true;
		}
		private function RayCastCallback(input:b2RayCastInput,proxyId:int):Number{
			var actor:Actor = m_tree.GetUserData(proxyId) as Actor;
			
			var output:b2RayCastOutput = new b2RayCastOutput;
			var hit:Boolean = actor.aabb.RayCast(output, input);
			
			if (hit)
			{
				m_rayCastOutput = output;
				m_rayActor = actor;
				m_rayActor.fraction = output.fraction;
				return output.fraction;
			}
			
			return input.maxFraction;
		}
		private function GetRandomAABB(aabb:b2AABB):void
		{
			var w:b2Vec2 = new b2Vec2;
			w.Set(2.0 * m_proxyExtent, 2.0 * m_proxyExtent);
			
			aabb.lowerBound.x = RandomFloat(-m_worldExtent, m_worldExtent);
			aabb.lowerBound.y = RandomFloat(0.0, 2.0 * m_worldExtent);
			aabb.upperBound = aabb.lowerBound.Add(w);
		}
		private function MoveAABB(aabb:b2AABB):void
		{
			var d:b2Vec2 = new b2Vec2;
			d.x = RandomFloat(-0.5, 0.5);
			d.y = RandomFloat(-0.5, 0.5);
			//d.x = 2.0f;
			//d.y = 0.0f;
			aabb.lowerBound.AddWith(d);
			aabb.upperBound.AddWith(d);
			
			var c0:b2Vec2 = aabb.lowerBound.Add(aabb.upperBound).Multiply(.5)
			var min:b2Vec2 = new b2Vec2(-m_worldExtent, 0.0);
			var max:b2Vec2 = new b2Vec2(m_worldExtent, 2.0 * m_worldExtent);
			var c:b2Vec2 = b2Math.Clamp(c0, min, max);
			
			aabb.lowerBound.AddWith(c.Subtract(c0))
			aabb.upperBound.AddWith(c.Subtract(c0));
		}
		private function CreateProxy():void
		{
			for (var i:int = 0; i < e_actorCount; ++i)
			{
				var j:int = Math.random() * e_actorCount;
				var actor:Actor = m_actors[j];
				if (actor.proxyId == b2DynamicTree.b2_nullNode)
				{
					GetRandomAABB(actor.aabb);
					actor.proxyId = m_tree.CreateProxy(actor.aabb, actor);
					return;
				}
			}
		}
		private function DestroyProxy():void
		{
			for (var i:int = 0; i < e_actorCount; ++i)
			{
				var j:int = Math.random() * e_actorCount;
				var actor:Actor = m_actors[j];
				if (actor.proxyId != b2DynamicTree.b2_nullNode)
				{
					m_tree.DestroyProxy(actor.proxyId);
					actor.proxyId = b2DynamicTree.b2_nullNode;
					return;
				}
			}
		}
		private function MoveProxy():void
		{
			for (var i:int = 0; i < e_actorCount; ++i)
			{
				var j:int = Math.random() * e_actorCount;
				var actor:Actor = m_actors [j];
				if (actor.proxyId == b2DynamicTree.b2_nullNode)
				{
					continue;
				}
				
				var aabb0:b2AABB = actor.aabb.Clone();
				MoveAABB(actor.aabb);
				var displacement:b2Vec2 = actor.aabb.GetCenter().Subtract(aabb0.GetCenter());
				m_tree.MoveProxy(actor.proxyId, actor.aabb, displacement);
				return;
			}
		}		
		private function Action():void
		{
			var choice:int = Math.random() * 20;
			
			switch (choice)
			{
				case 0:
					CreateProxy();
					break;
				
				case 1:
					DestroyProxy();
					break;
				
				default:
					MoveProxy();
			}
		}
		private function Query():void
		{
			m_tree.Query(QueryCallback,m_queryAABB);
			
			for (var i:int = 0; i < e_actorCount; ++i)
			{
				if (m_actors[i].proxyId == b2DynamicTree.b2_nullNode)
				{
					continue;
				}
				
				var overlap:Boolean = b2Collision.TestOverlap(m_queryAABB, m_actors[i].aabb);				
				b2Settings.b2Assert(overlap == m_actors[i].overlap);
			}
		}
		private function RayCast():void
		{
			m_rayActor = null;
			
			var input:b2RayCastInput = m_rayCastInput;
			
			// Ray cast against the dynamic tree.
			m_tree.RayCast(RayCastCallback, input);
			
			// Brute force ray cast.
			var bruteActor:Actor = null;
			var bruteOutput:b2RayCastOutput;
			for (var i:int = 0; i < e_actorCount; ++i)
			{
				if (m_actors[i].proxyId == b2DynamicTree.b2_nullNode)
				{
					continue;
				}
				
				var output:b2RayCastOutput = new b2RayCastOutput;
				var hit:Boolean = m_actors[i].aabb.RayCast(output, input);
				if (hit)
				{
					bruteActor = m_actors[i];
					bruteOutput = output;
					input.maxFraction = output.fraction;
				}
			}
			
			if (bruteActor != null)
			{
				b2Settings.b2Assert(bruteOutput.fraction == m_rayCastOutput.fraction);
			}
		}
		public override function Key(code:String):void
		{
			switch (code)
			{
				case 'a':
					m_automated = !m_automated;
					break;
				
				case 'c':
					CreateProxy();
					break;
				
				case 'd':
					DestroyProxy();
					break;
				
				case 'm':
					MoveProxy();
					break;
			}			
		}
		public override function Step():void
		{		
			super.Step();
			m_rayActor = null;
			for (var i:int = 0; i < e_actorCount; ++i)
			{
				m_actors[i].fraction = 1.0;
				m_actors[i].overlap = false;
			}
			
			if (m_automated == true)
			{
				var actionCount:int = Math.max(1, e_actorCount >> 2);
				
				for (i = 0; i < actionCount; ++i)
				{
					Action();
				}
			}
			
			Query();
			RayCast();
			var c:b2Color= new b2Color;
			for (i = 0; i < e_actorCount; ++i)
			{
				var actor:Actor = m_actors[i];
				if (actor.proxyId == b2DynamicTree.b2_nullNode)
					continue;
				
				c.Set(0.9, 0.9, 0.9);
				if (actor == m_rayActor && actor.overlap)
				{
					c.Set(0.9, 0.6, 0.6);
				}
				else if (actor == m_rayActor)
				{
					c.Set(0.6, 0.9, 0.6);
				}
				else if (actor.overlap)
				{
					c.Set(0.6, 0.6, 0.9);
				}
				
				m_debugDraw.DrawAABB(actor.aabb, c);
			}
			
			c.Set(0.7, 0.7, 0.7);
			m_debugDraw.DrawAABB(m_queryAABB, c);
			
			m_debugDraw.DrawSegment(m_rayCastInput.p1, m_rayCastInput.p2, c);
			
			var  c1:b2Color = new b2Color;
			c1.Set(0.2, 0.9, 0.2);
			var  c2:b2Color = new b2Color;
			c2.Set(0.9, 0.2, 0.2);
			m_debugDraw.DrawPoint(m_rayCastInput.p1, 3, c1);
			m_debugDraw.DrawPoint(m_rayCastInput.p2, 3, c2);
			
			if (m_rayActor)
			{
				var cr:b2Color = new b2Color;
				cr.Set(0.2, 0.2, 0.9);
				var p:b2Vec2 = m_rayCastInput.p1.Add(m_rayCastInput.p2.Subtract(m_rayCastInput.p1).Multiply(m_rayActor.fraction));
				m_debugDraw.DrawPoint(p, 3, cr);
			}
			
			{
				var height:Number = m_tree.GetHeight();
				writeMessage("dynamic tree height = " + height);
			}
		}
	}
}
import Box2D.Collision.b2AABB;

class Actor
{
	public var aabb:b2AABB=new b2AABB;
	public var fraction:Number;
	public var overlap:Boolean;
	public var proxyId:int;
}